/**
 * @file steering_servo.c
 * @brief Steering gear control for SG90 Servo Motor using GPIO and Pulse Width Modulation (PWM).
 * This file contains functions to control the SG90 servo motor's angle through pulse signals.
 * The servo motor is connected to GPIO2, and the control signals are generated with a 20ms period pulse,
 * where the high pulse width ranges from 0.5ms to 2.5ms for different angles.
 * 
 * @author Maker Studio
 * @license Apache 2.0
 * This code is open-sourced under the Apache 2.0 license. When using open-source libraries
 * or code, please ensure to acknowledge the original authors and sources.
 */

#include <stdio.h>
#include <stdlib.h>
#include <memory.h>
#include "pinctrl.h"
#include "common_def.h"
#include "soc_osal.h"
#include "test_suite_log.h"
#include "systick.h"

#include "ohos_init.h"
#include "cmsis_os2.h"
#include "iot_gpio.h"
#include <unistd.h>
#include "iot_pwm.h"
#include "iot_errno.h"
#include "iot_watchdog.h"
#include "../inc/iot_gpio_ex.h"

// According to the robot board schematic, the SG90 servo is connected to GPIO2 on the 3861.
// SG90 servo control requires the MCU to generate a 20ms period pulse signal,
// with a high pulse duration ranging from 0.5ms to 2.5ms to control the servo's rotation angle.

#define COUNT 10 // Approximate number of pulses needed to move the servo to the desired angle.
#define BSP_SG92R 2
#define FREQ_TIME 20000

/**
 * @brief Outputs a pulse signal with a high duration of `duty` microseconds and a total period of 20ms.
 * The signal has a high level for `duty` microseconds and a low level for the remaining period.
 * 
 * @param duty The duration (in microseconds) of the high pulse.
 */
void SetAngle(unsigned int duty)
{
    unsigned int time = FREQ_TIME;
    IoTGpioSetDir(BSP_SG92R, IOT_GPIO_DIR_OUT); // Set GPIO pin direction to output

    IoTGpioSetOutputVal(BSP_SG92R, IOT_GPIO_VALUE1); // Set GPIO pin to high
    uapi_systick_delay_us(duty); // Delay for the high pulse duration
    IoTGpioSetOutputVal(BSP_SG92R, IOT_GPIO_VALUE0); // Set GPIO pin to low
    uapi_systick_delay_us(time - duty); // Delay for the low pulse duration
}

/**
 * @brief Moves the steering gear to the center position (0°).
 * 1. Sets the high pulse duration to 1500 microseconds to achieve the center position.
 * 2. Sends multiple pulse signals to keep the servo centered.
 * @author Maker Studio
 * @license Apache 2.0
 */
void RegressMiddle(void)  // 0°
{
    unsigned int angle = 1500; // High pulse duration for center position
    for (int i = 0; i < COUNT; i++) {
        SetAngle(angle); // Continuously send pulses to maintain the center position
    }
}

/**
 * @brief Turns the steering gear 90 degrees to the right.
 * 1. Sets the high pulse duration to 500 microseconds to move the servo to the right.
 * 2. Sends multiple pulse signals to turn the servo to the right by 90 degrees.
 * @author Maker Studio
 * @license Apache 2.0
 */
void EngineTurnRight(void)  // -90°
{
    unsigned int angle = 500; // High pulse duration for 90° right turn
    for (int i = 0; i < COUNT; i++) {
        SetAngle(angle); // Continuously send pulses to move the servo to the right
    }
}

/**
 * @brief Turns the steering gear 45 degrees to the right.
 * 1. Sets the high pulse duration to 1000 microseconds to move the servo to 45° right.
 * 2. Sends multiple pulse signals to turn the servo to the right by 45 degrees.
 * @author Maker Studio
 * @license Apache 2.0
 */
void EngineTurnRight_half(void)  // -45°
{
    unsigned int angle = 1000; // High pulse duration for 45° right turn
    for (int i = 0; i < COUNT; i++) {
        SetAngle(angle); // Continuously send pulses to move the servo to the right by 45 degrees
    }
}

/**
 * @brief Turns the steering gear 45 degrees to the left.
 * 1. Sets the high pulse duration to 2000 microseconds to move the servo to 45° left.
 * 2. Sends multiple pulse signals to turn the servo to the left by 45 degrees.
 * @author Maker Studio
 * @license Apache 2.0
 */
void EngineTurnLeft_half(void)  // 45°
{
    unsigned int angle = 2000; // High pulse duration for 45° left turn
    for (int i = 0; i < COUNT; i++) {
        SetAngle(angle); // Continuously send pulses to move the servo to the left by 45 degrees
    }
}

/**
 * @brief Turns the steering gear 90 degrees to the left.
 * 1. Sets the high pulse duration to 2500 microseconds to move the servo to the left.
 * 2. Sends multiple pulse signals to turn the servo to the left by 90 degrees.
 *@author Maker Studio
 * @license Apache 2.0
 */
void EngineTurnLeft(void)  // 90°
{
    unsigned int angle = 2500; // High pulse duration for 90° left turn
    for (int i = 0; i < COUNT; i++) {
        SetAngle(angle); // Continuously send pulses to move the servo to the left
    }
}
